/***************************************************************************
 *  Filename: navadatareceiver.cpp
 *  Class: NavaDataReceiver
 *
 *  Copyright 2012  Mario Henrique Voorsluys
 *  <forcaeluz@gmail.com>
 *
 ****************************************************************************/


#include "navadatareceiver.h"
#include <QDebug>

NavDataReceiver::NavDataReceiver(QObject *parent) :
    QObject(parent)
{
    qDebug() << "[NavDataReceiver] Receiver Created";
    sequence = NAVDATA_SEQUENCE_DEFAULT-1;
    connect(&timer, SIGNAL(timeout()), this, SLOT(startRunning()));
    initializate();
}
NavDataReceiver::~NavDataReceiver(){
    receiverSocket->close();
}

void NavDataReceiver::initializate(){
    qDebug() << "[NavDataReceiver] Initializing";
    receiverSocket = new QUdpSocket(this);
    bool res = receiverSocket->bind(QHostAddress(WIFI_MYKONOS_IP), 5550, QUdpSocket::DefaultForPlatform);
    if(!res){
        qCritical(); << "[NavDataReceiver] Error binding: \n" << receiverSocket->errorString();
    }else{
        connect(receiverSocket, SIGNAL(readyRead()), this, SLOT(readData()));
        qDebug() << "[NavDataReceiver] Bound to: " << receiverSocket->localPort();
    }

    QHostAddress reciever;
    int res2;
    const char data[] = "\1\0\0\0";
    reciever = QHostAddress(WIFI_MYKONOS_IP);
    res2 = receiverSocket->writeDatagram(data,4,reciever,NAVDATA_PORT);

    if (res2 == -1) {
        qCritical(); << "[NavDataReceiver] Error initializing NavData UDP socket.";
    }else{
        qDebug() << "[NavDataReceiver] Sent initial package. The current state is: " << receiverSocket->state();
    }
    timer.start(10);
    qDebug() << "[NavDataReceiver] Initialized";
}

void NavDataReceiver::readData(){
    qDebug() << "[NavDataReceiver] Reading data";
    int size = receiverSocket->pendingDatagramSize();
    QByteArray tDatagram;
    tDatagram.resize(size);
    receiverSocket->readDatagram(tDatagram.data(), size);
    mutex.lock();
    datagram.append(tDatagram);
    if(datagram.size()>1){
        datagram.removeFirst();
    }
    droneresponded = true;
    mutex.unlock();
    qDebug() << "[NavDataReceiver] Finished Reading data";
}

void NavDataReceiver::startRunning(){
    qDebug() << "[NavDataReceiver] Spinning once";
    QByteArray tDatagram;
    mutex.lock();
    if(!datagram.isEmpty())
        tDatagram = datagram.takeFirst();
    qDebug() << "[NavDataReceiver] Messages queued" << datagram.size();
    mutex.unlock();
    int size = tDatagram.size();
    //Check if the datagram is not an empty one.
    if(size==0){
        qDebug() << "[NavDataReceiver] Empty datagram";
    }else{
        qDebug() << "[NavDataReceiver] Received datagram size: " << tDatagram.size();
        navdata_t *receivedData = (navdata_t*)tDatagram.constData();

        //The data header should be NAVDATA_HEADER
        if(receivedData->header == NAVDATA_HEADER){
            if(dronestate != receivedData->ardrone_state){
                dronestate = receivedData->ardrone_state;
                qDebug() << "[NavDataReceiver] Drone state changed";
                emit(updateState(dronestate));
            }
            if(get_mask_from_state(receivedData->ardrone_state, ARDRONE_NAVDATA_BOOTSTRAP)){
                if(receivedData->sequence > sequence){
                    emit initSequence(1);
                    booting = true;
                }else{
                    qDebug() << "[NavDataReceiver] Received data out of order";
                }

            }else if(get_mask_from_state(receivedData->ardrone_state, ARDRONE_COMMAND_MASK)){
                if(receivedData->sequence > sequence){
                    if(booting){
                        emit initSequence(2);
                        booting = false;
                    }
                }else{
                    qDebug() << "[NavDataReceiver] Received data out of order";
                }
            }
            if(receivedData->sequence > sequence){
                quint32 cks = 0;
                navdata_unpacked_5_t *unpackednavdata = new navdata_unpacked_5_t;
                bool res = ardrone_navdata_unpack_all_5(unpackednavdata, receivedData, &cks);
                qDebug() << "[NavDataReceiver] Unpacked.";
                if(res){
                    quint32 mCks = navdata_compute_cks((quint8*)tDatagram.data(), size-sizeof(navdata_cks_t));
                    if(mCks==cks){
                        float tRoll = unpackednavdata->navdata_demo.phi;
                        float tPitch = unpackednavdata->navdata_demo.theta;
                        float tYaw = unpackednavdata->navdata_demo.psi;
                        emit updateAngleData(tRoll, tPitch, tYaw);
                        float tVx = unpackednavdata->navdata_demo.vx;
                        float tVy = unpackednavdata->navdata_demo.vy;
                        float tVz = unpackednavdata->navdata_demo.vz;
                        emit updateVelocities(tVx, tVy, tVz);
                        int tAltitude = unpackednavdata->navdata_demo.altitude;
                        emit updateAltitude(tAltitude);
                        int tBattery = unpackednavdata->navdata_demo.vbat_flying_percentage;
                        emit updateBattery(tBattery);
                        qDebug() << "[NavDataReceiver] Data processing finished";
                    }else{
                        qDebug() << "[NavDataReceiver] Checksum failed.";
                    }
                }else{
                    qDebug() << "[NavDataReceiver] Unable to unpack";
                }
                delete(unpackednavdata);
            }else{
                qDebug() << "[NavDataReceiver] Received data out of order";
            }
        }else{
            qDebug() << "[NavDataReceiver] Header is not NAVDATA_HEADER";
        }
    }
}
